/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-08-28     huger       the first version
 */

#include "pid.h"
#include "Foc/foc.h"
#include "App/bldc_motor.h"
#include "Magnetic/magnetic.h"

CURRENT_PID_DEF Current_D_PID;
CURRENT_PID_DEF Current_Q_PID;
SPEED_PID_DEF Speed_Pid;
POSI_PID_DEF  Posi_Pid;


void PID_Init(void)
{
//    // Id- loop
//    Current_D_PID.P_Gain = -0.5;
//    Current_D_PID.I_Gain = -0.0025;
//    Current_D_PID.D_Gain = 0;
//    Current_D_PID.Max_Output = 12;
//    Current_D_PID.Min_Output = -12;
//
//    //Iq- loop
//    Current_Q_PID.P_Gain = -0.5;
//    Current_Q_PID.I_Gain = -0.0025;
//    Current_Q_PID.D_Gain = 0;
//    Current_Q_PID.Max_Output = 12;
//    Current_Q_PID.Min_Output = -12;
//
//    //Speed loop
//    Speed_Pid.P_Gain = 0.1;
//    Speed_Pid.I_Gain = 0.000;
//    Speed_Pid.D_Gain = 0;
//    Speed_Pid.Max_Output = 300;
//    Speed_Pid.Min_Output = -300;
//
//
//    //Posi loop
//    Posi_Pid.P_Gain = 0.075;
//    Posi_Pid.I_Gain = 0.0;
//    Posi_Pid.D_Gain = 0;
//    Posi_Pid.Max_Output = 300;
//    Posi_Pid.Min_Output = -300;


    // Id- loop
    Current_D_PID.P_Gain = -0.25;
    Current_D_PID.I_Gain = 0;
    Current_D_PID.D_Gain = 0;
    Current_D_PID.Max_Output = 12;
    Current_D_PID.Min_Output = -12;

    //Iq- loop
    Current_Q_PID.P_Gain = -0.25;
    Current_Q_PID.I_Gain = 0;
    Current_Q_PID.D_Gain = 0;
    Current_Q_PID.Max_Output = 12;
    Current_Q_PID.Min_Output = -12;

    //Speed loop
    Speed_Pid.P_Gain = 0.051;//0.2
    Speed_Pid.I_Gain = 0.0051;   //0.002
    Speed_Pid.D_Gain = 0;
    Speed_Pid.Max_Output = 200;
    Speed_Pid.Min_Output = -200;


    //Posi loop
    Posi_Pid.P_Gain = 0.125;//0.025
    Posi_Pid.I_Gain = 0.0;//
    Posi_Pid.D_Gain = 0;
    Posi_Pid.Max_Output = 200;
    Posi_Pid.Min_Output = -200;
}


/***************************************
current PID
B is Integral windup gain ,
usually, it is about I gain
***************************************/

void Current_PID_Calc(float_t ref_temp,float_t fdb_temp,float_t* out_temp,CURRENT_PID_DEF* current_pid_temp)
{
    float_t temp;
    current_pid_temp->err =  ref_temp - fdb_temp;

    /* 限定闭环死区 */
//    if((current_pid_temp->err >= - 0.2) && (current_pid_temp->err  <= 0.2))
//    {
//        current_pid_temp->err = 0;
//        current_pid_temp->integral = 0;
//    }

    current_pid_temp->integral += current_pid_temp->err;

        /*PID算法实现*/
    temp = current_pid_temp->P_Gain * current_pid_temp->err +
            current_pid_temp->I_Gain * current_pid_temp->integral +
            current_pid_temp->D_Gain * (current_pid_temp->err - current_pid_temp->err_last);

        /*误差传递*/
    current_pid_temp->err_last = current_pid_temp->err;

    if (temp > current_pid_temp->Max_Output) {
        *out_temp = current_pid_temp->Max_Output;
    } else if (temp < current_pid_temp->Min_Output) {
        *out_temp = current_pid_temp->Min_Output;
    } else {
        *out_temp = temp;
    }
}

void Speed_Pid_Calc(float_t ref_temp,float_t fdb_temp,float_t* out_temp,SPEED_PID_DEF* current_pid_temp)
{
    float_t temp;

    current_pid_temp->err =  ref_temp - fdb_temp;


    /* 限定闭环死区 */
    if((current_pid_temp->err >= - 2) && (current_pid_temp->err  <= 2))
    {
        current_pid_temp->err = 0;
//        current_pid_temp->integral = 0;
    }

    current_pid_temp->integral += current_pid_temp->err;

        /*PID算法实现*/
    temp = current_pid_temp->P_Gain * current_pid_temp->err +
            current_pid_temp->I_Gain * current_pid_temp->integral +
            current_pid_temp->D_Gain * (current_pid_temp->err - current_pid_temp->err_last);

        /*误差传递*/
    current_pid_temp->err_last = current_pid_temp->err;

    if (temp > current_pid_temp->Max_Output) {
        *out_temp = current_pid_temp->Max_Output;
    } else if (temp < current_pid_temp->Min_Output) {
        *out_temp = current_pid_temp->Min_Output;
    } else {
        *out_temp = temp;
    }
}


void Posi_Pid_Calc(float_t ref_temp,float_t fdb_temp,float_t* out_temp,POSI_PID_DEF* current_pid_temp)
{
    float_t temp;

    current_pid_temp->err =  ref_temp - fdb_temp;


    /* 限定闭环死区 */
    if((current_pid_temp->err >= - 0.05) && (current_pid_temp->err  <= 0.05))
    {
        current_pid_temp->err = 0;
        current_pid_temp->integral = 0;
    }
    current_pid_temp->integral +=  current_pid_temp->err ;

    temp =  current_pid_temp->P_Gain * current_pid_temp->err +
            current_pid_temp->I_Gain * current_pid_temp->integral +
            current_pid_temp->D_Gain * (current_pid_temp->err-current_pid_temp->err_last);

    current_pid_temp->err_last = current_pid_temp->err;

    if (temp > current_pid_temp->Max_Output) {
        *out_temp = current_pid_temp->Max_Output;
    } else if (temp < current_pid_temp->Min_Output) {
        *out_temp = current_pid_temp->Min_Output;
    } else {
        *out_temp = temp;
    }
}




static int motor_current_Q_pid_set(int argc, char *argv[])
{
    Current_Q_PID.err=0;
    Current_Q_PID.err_last=0;
    Current_Q_PID.integral=0;
    Current_Q_PID.P_Gain = atof(argv[1]);
    Current_Q_PID.I_Gain = atof(argv[2]);
    Current_Q_PID.D_Gain = atof(argv[3]);
    Current_Q_PID.Max_Output = atof(argv[4]);
    Current_Q_PID.Min_Output = atof(argv[5]);
    rt_kprintf("kp:%f,ki:%f,kd:%f,max:%f,min:%f\r\n",Current_Q_PID.P_Gain,Current_Q_PID.I_Gain,Current_Q_PID.D_Gain,Current_Q_PID.Max_Output,Current_Q_PID.Min_Output);
    return RT_EOK;
}
MSH_CMD_EXPORT(motor_current_Q_pid_set, num1:kp  num2:ki  num3:kd   num4:max  num5:min);

static int motor_current_D_pid_set(int argc, char *argv[])
{
    Current_D_PID.err=0;
    Current_D_PID.err_last=0;
    Current_D_PID.integral=0;
    Current_D_PID.P_Gain = atof(argv[1]);
    Current_D_PID.I_Gain = atof(argv[2]);
    Current_D_PID.D_Gain = atof(argv[3]);
    Current_D_PID.Max_Output = atof(argv[4]);
    Current_D_PID.Min_Output = atof(argv[5]);
    rt_kprintf("kp:%f,ki:%f,kd:%f,max:%f,min:%f\r\n",Current_D_PID.P_Gain,Current_D_PID.I_Gain,Current_D_PID.D_Gain,Current_D_PID.Max_Output,Current_D_PID.Min_Output);
    return RT_EOK;
}
MSH_CMD_EXPORT(motor_current_D_pid_set, num1:kp  num2:ki  num3:kd   num4:max  num5:min);


static int motor_spd_pid_set(int argc, char *argv[])
{
    Speed_Pid.err=0;
    Speed_Pid.err_last=0;
    Speed_Pid.integral=0;
    Speed_Pid.P_Gain = atof(argv[1]);
    Speed_Pid.I_Gain = atof(argv[2]);
    Speed_Pid.D_Gain = atof(argv[3]);
    Speed_Pid.Max_Output = atof(argv[4]);
    Speed_Pid.Min_Output = atof(argv[5]);

    Speed_Ref = atof(argv[6]);

    Motor_Run_Mode = spd;
    rt_kprintf("kp:%f,ki:%f,kd:%f,max:%f,min:%f,spd:%f\r\n",Speed_Pid.P_Gain,Speed_Pid.I_Gain,Speed_Pid.D_Gain,Speed_Pid.Max_Output,Speed_Pid.Min_Output,Speed_Ref);
    return RT_EOK;
}
MSH_CMD_EXPORT(motor_spd_pid_set, num1:kp  num2:ki  num3:kd   num4:max  num5:min   num6:target_spd);


static int motor_pos_pid_set(int argc, char *argv[])
{
    Posi_Pid.err=0;
    Posi_Pid.err_last=0;
    Posi_Pid.integral=0;
    Posi_Pid.P_Gain = atof(argv[1]);
    Posi_Pid.I_Gain = atof(argv[2]);
    Posi_Pid.D_Gain = atof(argv[3]);
    Posi_Pid.Max_Output = atof(argv[4]);
    Posi_Pid.Min_Output = atof(argv[5]);

    Posi_Ref = magnetic_current_pos + atof(argv[6])*cpr;
    Speed_Ref = atof(argv[7]);
    Motor_Run_Mode = pos;
    rt_kprintf("kp:%f,ki:%f,kd:%f,max:%f,min:%f,target_pos:%f,spd:%f\r\n",Posi_Pid.P_Gain,Posi_Pid.I_Gain,Posi_Pid.D_Gain,Posi_Pid.Max_Output,Posi_Pid.Min_Output,Posi_Ref,Speed_Ref);
    return RT_EOK;
}
MSH_CMD_EXPORT(motor_pos_pid_set, num1:kp  num2:ki  num3:kd   num4:max  num5:min  num6:pos num7:spd);




